#pragma once

#include <opencv2\opencv.hpp>

class HeadCamera
{
public:

	double focal;
	cv::Point2d center;

	cv::Mat distortion;

	cv::Vec6f pos;

public:
	HeadCamera()
	{

	}

	cv::Vec3d Reproject(int u, int v)
	{
		cv::Mat K = (cv::Mat_<double>(3, 3) << focal, 0, center.x, 0, focal, center.y, 0, 0, 1);

		cv::Vec2d pu;
		cv::undistortPoints(cv::Vec2d(u, v), pu, K, distortion);

		return cv::Vec3d((pu[0] - center.x) / focal, (pu[1] - center.y) / focal, 1);
	}
	
	static cv::Vec3d TransformTo(cv::Vec3d p, cv::Vec6f tf)
	{
		cv::Matx33d rm;
		cv::Rodrigues(cv::Vec3d(tf[3], tf[4], tf[5]), rm);
		return cv::Vec3d(tf[0], tf[1], tf[2]) + rm*p;
	}

	cv::Vec3d CameraToHead(cv::Vec3d p)
	{
		return TransformTo(p, pos);
	}

	void RayAtBase(cv::Vec3d ray[2], int u, int v, cv::Vec6f armPos)
	{
		cv::Vec3d rayHead[2];
		rayHead[0] = cv::Vec3d(pos[0], pos[1], pos[2]);
		rayHead[1] = CameraToHead(Reproject(u, v));
		ray[0] = TransformTo(rayHead[0], armPos);
		ray[1] = TransformTo(rayHead[1], armPos);
	}


	int RayIntersectionAtBase(cv::Vec3f &intersection, int u, int v, cv::Vec6f armPos, cv::Vec4f desk)
	{
		cv::Vec3d rayBase[2];
		RayAtBase(rayBase, u, v, armPos);

		cv::Vec3d rayDirection = rayBase[1] - rayBase[0];

		cv::Vec3d desk1(desk[0], desk[1], desk[2]);

		double a = rayDirection.ddot(desk1);

		double b = rayBase[0].ddot(desk1) + desk[3];

		if (a != 0)
		{
			double k = -b / a;
			intersection = rayBase[0] + k*rayDirection;
			return 1;
		}

		if (b == 0)
		{
			return -1;
		}

		return 0;
	}

	static void ChessBoardPos(std::vector<cv::Point3f> &posCB, int nx = 6, int ny = 9, float l = 0.04115)
	{
		posCB.assign(nx*ny, cv::Point3f(0,0,0));
		for (int i = 0;i < ny;i++)
		{
			for (int j = 0;j < nx;j++)
			{
				int idx = j + i*nx;

				posCB[idx].x = j*l;
				posCB[idx].y = (ny-1-i)*l;
			}
		}
	}

	static cv::Vec6f InverseTransform(cv::Vec6f tf)
	{
		cv::Matx33f R;
		cv::Rodrigues(cv::Vec3f(tf[3], tf[4], tf[5]), R);

		cv::Matx33f Ri = R.inv();

		cv::Vec3f ri;
		cv::Rodrigues(Ri, ri);

		std::cout << "ri=" << ri << '\n';
		std::cout << "tf=" << tf << '\n';
		std::cout << "cross=" << cv::norm(ri.cross(cv::Vec3f(tf[3], tf[4], tf[5]))) << '\n';

		cv::Vec3f ti = -Ri*cv::Vec3f(tf[0], tf[1], tf[2]);

		return cv::Vec6f(ti[0], ti[1], ti[2], ri[0], ri[1], ri[2]);



	}

	static cv::Vec6f ComposeRT(cv::Vec6f tf1to2, cv::Vec6f tf2to3)
	{
		cv::Vec3f t1to3, r1to3;
		cv::composeRT(cv::Vec3f(tf1to2[3], tf1to2[4], tf1to2[5]), cv::Vec3f(tf1to2[0], tf1to2[1], tf1to2[3]),
			cv::Vec3f(tf2to3[3], tf2to3[4], tf2to3[5]), cv::Vec3f(tf2to3[0], tf2to3[1], tf2to3[3]),
			r1to3, t1to3);
		return cv::Vec6f(t1to3[0], t1to3[1], t1to3[2], r1to3[0], r1to3[1], r1to3[2]);
	}

	void Calibrate(const std::vector<cv::Vec6f> &headToBase, cv::Vec6f deskToBase, const std::vector<std::vector<cv::Point2f>> &corners, cv::Size imageSize, cv::Size patternsize, float l = 0.04115)
	{
		std::vector<std::vector<cv::Point3f>> posCB(corners.size(), std::vector<cv::Point3f>());
		for(size_t i=0;i<posCB.size();i++)
			ChessBoardPos(posCB[i], patternsize.width, patternsize.height, l);

		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
		std::vector<cv::Mat> Rv, Tv;

		distortion.create(5, 1, CV_64FC1);
		distortion.setTo(0);


		double rms = cv::calibrateCamera( posCB, corners, imageSize, K,
			distortion, Rv, Tv, cv::CALIB_FIX_ASPECT_RATIO);

		printf("RMS error reported by calibrateCamera: %g\n", rms);

		std::cout << "K=" << K << '\n';
		std::cout << "distortion=" << distortion << '\n';

		std::cout << "Rv=" << Rv[0] << '\n';
		std::cout << "Tv=" << Tv[0] << '\n';
		std::cout << "headToBase=" << headToBase[0] << '\n';

		cv::Vec6f baseToHead = InverseTransform(headToBase[0]);

		cv::Vec6f camToDesk = InverseTransform(cv::Vec6f(Tv[0].at<double>(0), Tv[0].at<double>(1), Tv[0].at<double>(2), Rv[0].at<double>(0), Rv[0].at<double>(1), Rv[0].at<double>(2)));

		cv::Vec6f camToBase = ComposeRT(camToDesk, deskToBase);

		cv::Vec6f camToHead= ComposeRT(camToBase, baseToHead);

		pos = camToHead;

		focal = K.at<double>(0, 0);
		center.x = K.at<double>(0, 2);
		center.y = K.at<double>(1, 2);

	}


};
